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基于 图 优化 的 视觉 SLAM 研究 进展 与 应 用 分 析 C 


王 录 涛 ， 吴 林峰 
(成 都 信息 工程 大 学 计算 机 学 院 , 成 都 610225) 


摘 X: 同步 定位 与 构图 技术 (simultaneous localization and mapping，SLAM) 指 机 器 人 在 构建 环境 地 图 的 同时 对 自己 的 
运动 状态 进行 估计 ， 是 实现 未 知 环境 下 机 器 人 全 自主 运动 的 核心 。 为 了 对 SLAM 技术 有 更 为 全 面 的 把 握 ， 在 回顾 过 去 
三 十 年 里 视觉 SLAM 技术 发 展 历程 基础 上 ， 详 细 分 析 了 视觉 SLAM 问题 的 本 质 与 求解 的 复杂 性 。 重 点 对 在 提高 位 姿 
估计 精度 、 构 建 全 局 一 致 地 图 与 提升 算法 求解 效率 上 的 最 新 研究 成 果 进 行 了 介绍 ， 并 对 当前 代表 性 的 算法 实现 方案 进 
行 了 分 析 与 比较 。 针 对 未 来 大 尺度 环境 、 全 生命 周期 应 用 需求 ， 对 现 有 算法 框架 的 不 足 与 最 新 研究 趋势 进行 了 归纳 总 
结 。 最 后 ， 探 讨 了 深度 学 习 技 术 与 视觉 SLAM 问题 求解 的 关联 性 
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Application analyses and research progress of graph-based visual SLAM 


I Wang Lutao, Wu Linfeng 
(School of Computer Science, Chengdu University of Information Technology, Chengdu 610225, China) 


Abstract: Simultaneous localization and mapping consists in the concurrent construction of a model of environment, and the 
estimation of the robot moving within it. It is one of the key problems for the robot to be completely autonomous in unknown 
environments. In order to give a comprehensive understanding of SLAM, we first give an overview of the progress of visual 


SLAM community has made over the last 30 years in this survey. We then present the non-linearity of the mathematical model 


pl. and computational complexity in visual SLAM algorithms. We mostly focus on the latest achievements and approaches 


improving the accuracy of pose estimation, building a globally consistency representation of the environment and promoting 


computation efficiency. We also survey the failure modes of current visual SLAM algorithms for large scale, full lifecycle 
implementation and the different ways to address that. Finally, we discuss the potential connections between deep learning 
architectures and visual SLAM state estimations. 


Key words: simultaneous localization and mapping; graph optimization; data association; sparsification; deep learning 


问题 。Smith 等 外 进行 了 开创 性 的 研究 ， 首 先 将 SLAM 问题 系 
统 地 表示 为 随机 估计 问题 。 机 器 人 位 姿 和 地 标 被 看 做 服从 某 种 
为 实现 未 知 环境 下 全 自主 运动 ， 移 动机 器 人 必须 解决 环境 分 布 的 随机 变量 ， 利 用 运动 数据 和 观测 数据 ， 采 用 滤波 理论 实 
感知 与 自身 定位 两 个 基本 的 问题 。 同 步 定 位 与 地 图 构建 技术 现 系统 状态 预测 与 观测 更 新 ， 同 时 实现 地 图 的 在 线 更 新 ， 有 具有 
(Simultaneous localization and mapping，SLAM) 将 机 器 人 定位 与 较 好 的 实时 性 ,代表 性 算法 有 扩展 卡尔 曼 滤波 (Extended Kalman 
到 构建 融 为 一 体 ， 使 机 器 人 在 缺乏 环境 先 验 信息 的 情况 下 ， Filters, EKF)DB]、 扩 展 信 息 滤波 内 、UKFG、Rao-Balckwellized fs 
能 依据 自身 搭载 的 传感器 在 运动 过 程 中 增 量 获取 未 知 环境 的 特 子 滤波 四、FastSlam[7] 等 。 滤 波 方法 假设 状态 估计 的 Markov 性 ， 
征 信息 , 同时 实现 自我 运动 轨迹 的 精确 估计 趾 ,。 相对 激光 SLAM 仅 用 相 邻 帧 信息 对 机 器 人 状态 进行 估计 ， 难 以 处 理 当 前 帧 与 历 
技术 ,视觉 SLAM 系统 硬件 成 本 低 、 能 获取 环境 纹理 与 色彩 信 史 帧 的 数据 关联 问题 。SLAM 系统 运动 方程 和 观测 方程 是 非 线 
E, KAMEA SEE HIMA BUE. 性 函数 ， 滤 波 方法 采用 一 阶 泰勒 近似 计算 状态 的 后 验 概率 ， 当 
迄今 为 止 , 视觉 SLAM 技术 的 发 展 过 程 可 归纳 为 两 个 阶段 。 系统 存在 强烈 的 非 线性 时 ， 大 的 线性 化 估计 误差 将 不 可 避免 。 
第 一 阶段 从 1986 年 到 2004 年 ,采用 贝 叶 斯 滤波 技术 求解 SLAM 此 外 ,系统 参数 和 观测 存在 的 不 确定 性 , 也 会 造成 误差 的 累积 ， 
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导致 构建 地 图 的 不 一 致 性 。 在 算法 实现 上 ， 滤 波 方法 需 存储 、 


维护 和 更 新 状态 变量 的 均值 和 方差 ， 且 存储 容量 与 待 估计 状态 


变量 呈 平 方 关系 。 因 此 ， 滤 波 方法 仅 适 用 于 计算 资源 受 限 ， 或 


估计 变量 较 少 的 情况 下 。Paz 等 人 采用 子 地 
EKF 的 工作 环境 尺度 问题 ， 但 仅 能 在 100 米 


[8], 


第 二 阶段 (2004-2016) 致 力 于 SLAM # 
研究 ， 关 于 SLAM 问题 的 可 观测 性 、 收 敛 性 与 一 致 性 
量 见 诸 于 报道 。 降 低 对 计算 资源 的 需求 ， 实 现 系统 实时 性 工作 


的 同时 扩展 应 用 环境 
多 化 技术 ， 构 建 针 对 观测 方程 的 非 线 


| 


图 分 割 的 方法 解决 


尽 度 成 为 SLAM 应 用 的 关键 中， 而 利 / 
性 最 小 二 乘 目 标 函 数 ， 将 


Marquardt IRE 


精度 , 成 为 SLAM 研究 的 主流 。 同 时 ，SLAM 问 
使 得 全 局 一 致 解 的 获取 成 为 可 能 。 


逐渐 被 认 知 ， 


机 器 人 位 姿 与 路 标点 
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作为 待 优化 变量 
代 估 计 最 优 解 , 提高 


» RIA 


的 范围 


E 论 分 析 与 实现 细节 
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两 部 分 ， 如 图 2 所 示 。 前 端 一 方面 根 拉 


征 ， 
的 
键 ， 


P: Ui 为 运动 传感器 输入 ， Wi 为 状态 噪声 ， 6, 为 观测 
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根据 SLAM 数学 模型 ， 可 将 SLAM 系统 分 为 前 端 与 后 端 


实现 观测 与 路 标 、 以 及 位 姿 的 数据 关联 ， 另 一 方 国 


昌 传 感 器 数据 提取 环境 特 
| 为 后 端 


FE 线性 优化 估计 提供 可 靠 初 始 值 。 数 据 关 联 是 前 端 处 理 的 关 
既 要 解决 连续 测量 帧 的 特征 关联 ， 实 现 运动 跟踪 ， 也 要 负 


的 研究 大 


JE 


F 顿 法 、Levenberg- 


局 部 定 


上 与 路 标点 的 估计 
TRA B n E tE 


的 工作 环境 尺度 


差 的 SLAM RACAL ME, 


经 过 三 十 多 年 的 发 


A 


法 定位 精度 、 


运动 轨迹 估计 漂移 、 全 


展 ， 视 觉 SLAM 理论 


局 一 致 性 环境 


目前 , 在 50km 


有 1% 平 移 误差 与 0.003 deg m 旋转 误 


框架 趋 于 稳定 ， 算 
图 构建 等 


关键 性 问题 在 计算 资源 、 运 动 模型 、 静 态 环 境 与 性 能 等 约束 条 


牛 得 以 满足 的 情况 下 ， 得 到 了 有 效 解决 ， 但 


大 尺度 复杂 


自 适 应 应 | 
有 效 的 解决 方案 。 


的 问题 、 


术 进 行 了 比较 分 析 。Cadena 
地 图 表示 方法 与 研究 进 
证 , 并 对 相关 前 沿 课题 进行 了 详细 


` 境 下 的 高 性 能 、 低 失效 率 应 


针对 全 生命 周期 、 


]， 以 及 计算 资源 的 


与 构建 基于 任务 驱动 的 感知 模 


Liu 等 0 对 近期 代表 性 的 


RE 


自动 驾驶 应 | 


中 存在 的 问题 与 
本 文 侧重 对 视觉 SLAM 技术 的 理论 基础 
尺度 环境 工作 存在 的 问题 及 


型 等 方面 还 缺乏 更 为 


和 目 视 觉 SLAM 技 


0 总结 了 SLAM 的 状态 估计 存在 
展 ， 以 及 SLAM 性 


能 的 理论 保 


论述 。Cuillaume 03] 对 SLAM 
解决 途径 进行 了 分 析 。 


与 全 生命 周期 、 大 


了 SLAM 系统 处 3 


里 架构 , 结合 理论 基础 分 析 


在 的 问题 ， 


系统 全 生命 周期 、 


存在 的 问题 及 解决 途径 ， 并 对 古 


对 最 新 研究 进 
阶段 的 主流 算法 及 可 能 达到 


的 性 能 指标 。 


解决 途径 进行 深入 剖析 ， 首 先 给 出 
图 
展 进行 了 阐述 ， 然 后 归纳 总 结 了 现 
最 后 , 针对 未 来 SLAM 


优化 估计 方法 存 


开放 大 尺度 工作 场景 下 的 应 用 需求 ， 探 讨 了 


1 SLAM 数学 模型 


SLAM 技术 可 表述 为 在 未 知 环境 


传感器 获得 的 环境 特征 


身 位 姿 的 精确 估计 ， 如 图 1 所 示 。 


在 图 
轨迹 ， 
的 观测 产生 


Vie Y HEKER. IY 
的 观测 数据 Zi.; SLAM 问题 模 
型 和 观测 模型 表示 


EE = f (Xi us W,) 
£y) 


Zi = hly, X, 


f 究 趋势 进行 了 


展望 。 


FPF， 移动 机 器 人 利用 搭载 


悍 息 增 量 式 构建 环境 地 图 ， 同 时 实现 自 


1 中 ， 机 器 人 X 个 时 刻 的 位 姿 X%…,%, 构成 自身 运动 
是 大 时 刻 的 位 姿 X 与 在 X SEES Y'; 


型 可 


zzi 


]—4 38] bà 


(1) 


责 当 前 测量 与 历史 测量 的 关联 ， 完 成 定位 与 构图 功能 。 后 端 则 


性 地 图 构建 ， 改 善 定位 精度 估计 。 


1 SLAM 问题 示意 图 


Fig.l Illustration of SLAM problem 


传感器 数据 前 端 front-end 后 端 back-end 


视觉 里 程 计 


闭环 检测 
(Loop Closing) | | / 


图 2 SLAM 系统 框架 


Fig.2 Architecture of SLAM system 


图 3 给 出 了 后 端 优化 处 理 前 后 运动 轨迹 估计 对 比 ， 采 月 


在 前 端 基础 上 进行 位 姿 与 路 标点 的 推理 与 优化 ， 实 现 全 局 一 致 


HAY 


数据 集 为 MIT Killian Court. A] 3(a) MIT Killian Court 光学 照 
片 与 机 器 人 运动 路 径 标 注 。 由 于 传感器 测量 误差 与 机 器 人 位 姿 
估计 累积 误差 ， 机 器 人 运动 轨迹 估计 结果 与 实际 标准 值 存在 较 


大 偏差 ， 而 后 端 优化 则 可 以 对 估计 误差 进行 修正 ， 使 运动 人 
估计 与 实际 运动 轨迹 更 为 相符 。 


(a) MIT Killian Court 光学 照片 与 机 器 人 运动 轨迹 标注 
(a) Optical map of MIT Killian Court with trajectory annotation 


eX | 
i 
$f ig ^i 
| 
d - 


(b) 未 经 后 端 优化 处 理 (CN DU LG Rb BE JE 
(b)without optimization (c)with optimization 
图 3 后 端 优化 处 理 前 后 运动 轨迹 估计 对 比 


Estimated trajectory with or without back-end optimization 


Fig.3 
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2 基于 图 优化 的 SLAM 问题 求解 

2.4 SLAM 问题 描述 

在 有 噪声 的 观测 数据 中 精确 估计 状态 变量 是 SLAM 的 基 
本 问题 。 图 优化 技术 将 机 器 人 轨迹 与 路 标 联 合 


X-[X, X Nore] 作为 待 估计 变量 ， 在 给 定 观测 数据 
Z={Zk=1…m)} 的 情况 下 估计 xX， 使 得 后 验 概率 分 布 


P(x|z) 最 大 。 根据 贝 叶 斯 法 则 , 求解 最 大 后 验 概率 等 于 最 大 化 
似 然 P(z|X) 和 先 验 概率 P(X) 的 乘积 


Xyap = arg max P(X|Z)=argmaxP(z|X)P(X) (2) 


在 没有 先 验 信息 的 情况 下 , PCO 为 常数 ，Xmr 简化 为 最 大 
似 然 概 率 估计 (Maximum Likelihood Estimation, MLE) 


X' =argmaxP(Zz| X) 6) 


在 线性 高 斯 假设 下 , 图 优化 方法 与 KF 方法 估计 结果 一 致 ， 
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Q2, x6) RHABPEIBUE SR Sg 


X -argmine! Qe 
BY (7) 


对 于 式 (6) 所 表示 的 非 线 性 最 小 二 乘 优化 问题 ， 一 般 可 采用 
和 迭代 线性 化 方式 求解 。 在 给 定 初 值 % 时 ,对 误差 函数 e(X) 在 Xo 
附近 进行 一 阶 泰勒 展开 ， 求 关于 % 增 量 Ox 的 导数 并 使 其 等 于 
零 ， 得 到 增 量 方程 


Hôx=g (8) 
其 中 ，9=-JOOreOO 73 RRE RAAB, H= 为 
Hessian 4B, JO) = 0e/ 0x JIPET EERE. E, ARBAB 
的 解 为 : 


X =X TOX (9) 

Gauss-Newton 法 对 上 述 过 程 进行 迭代 求解 直到 收 化 ,可 获 

得 参量 的 最 优 估 计 。 为 避免 Sx 过 大 导致 的 近似 误差 增加 ， 
Levenberg-Marquardt 算法 通过 对 6x 添 加 置信 域 ， 引 入 松弛 因 


lim] 


但 对 于 非 线性 非 高 斯 系统 ， 图 优化 技术 将 位 姿 与 路 标 变量 作为 
一 个 整体 ， 使 用 所 有 时 刻 采 和 集 数据 进行 联合 估计 ， 克 服 了 EKF 
的 线性 化 误差 与 噪声 高 斯 分 布 假设 ， 成 为 当今 研究 的 主流 。 在 
计算 资源 允许 的 条 件 下 ， 图 优化 方法 也 成 为 优选 方法 。 

假设 观测 数据 乙 是 独立 的 ， 最 大 后 验 概率 的 估计 可 分 解 为 
各 次 观测 最 大 似 然 的 乘积 


X,» = arg max P(x)] [Pz | x) 
x k=l 


. (4) 
= arg max POO] [PG | X) 
x k=l 


其 中 : X% SX。 假设 传感器 受 高 斯 白 噪声 影响 , 观测 值 服从 高 斯 
分 布 ， 协 方差 矩阵 为 2 ， 则 最 大 测量 似 然 可 表示 为 


1 
P(z, | X,) c ew(-; Ih, OX) -Zl ) (5) 


将 式 (5) 代 入 式 (4), 根据 最 大 化 后 验 概率 等 于 最 小 化 负 对 数 
似 然 函 数 ， 最 大 后 验 概率 的 估计 可 进一步 显示 的 表述 为 求解 机 
器 人 位 姿 与 路 标的 联合 估计 ， 使 得 估计 结果 与 观测 的 误差 平方 
函数 最 小 


X = argmax—log( Poo] [Pz | x) 
A D (6) 
-argmin “Hh, - 2b, 
其 中 :及 (*) 是 抽象 的 非 线性 函数 , 可 用 以 表示 惯性 传感器 、 编 码 
器 、GPS、 相 机 等 数学 模型 。 当 噪声 不 满足 标准 正 态 分 布 时 ， 式 
(6) 中 的 度量 误差 4 可 用 针 等 其 他 范 数 取代 ， 为 增加 系统 的 稳健 
型 ,减少 对 外 点 的 敏感 度 ,， 也 可 用 Huber, Tukey 等 损失 函数 代 


9 LR 


为 方便 表述 ， 令 e00-h(X)-z , e=[el-.er] 表示 


m 维 误差 向 量 ， Qsdiag(Q-. OU) 为 误差 权重 和 矩阵， 


m 


子 4 有 效 改善 了 5X 求 解 的 稳定 性 ; 
(H+ A)óx 2g (10) 


2.2 算法 本 质 分 析 

SLAM 问题 中 的 位 姿 估计 由 平移 和 旋转 估计 两 部 分 构成 。 
其 中 , 旋转 计算 的 非 线 性 决定 了 SLAM 本 质 是 非 线性 优化 问题 。 
此 外 ， 非 高 斯 噪声 、 变 量 维 数 巨大 ， 以 及 静态 场景 假设 往往 得 
不 到 满足 等 多 种 因素 的 存在 ，SLAM 优化 问题 的 目标 函数 (7) 异 
常 复 杂 ， 位 姿 与 路 标的 估计 极 易 陷入 局 部 极 小 ， 而 位 姿 与 路 标 
的 错误 估计 将 使 得 机 器 人 定位 产生 漂移 ， 无 法 获得 全 局 一 致 的 
运动 轨迹 估计 与 地 图 构建 ， 从 而 难以 满足 导航 、 环 境 重建 等 应 
用 需要 。 图 4 给 出 了 球体 和 圆 环 全 局 优化 估计 与 估计 陷入 局 部 
极 小 时 的 仿真 对 比 。 


sphere-a torus 


Optimum 


Local Minimum 


图 


A 
H 


局 优化 估计 与 局 部 优化 估计 仿真 对 比 


Fig.4 Trajectory estimates resulting from global optimum and convergence to 
local minima 

文献 [14] 利 用 视觉 里 程 计 与 3D 激光 里 程 计 的 组 合 获取 用 
于 优化 估计 的 初始 值 ， 在 KITTI 数据 集 上 测试 ， 平 移 计算 精度 
达到 0.68%， 旋 转 误 差 达 到 0.0016 deg-m™ 。 通 过 对 观测 特征 点 
的 第 选 与 跟踪 ， 应 用 多 帧 图 像 特征 点 绝对 差异 求 和 、 归 一 化 互 
相关 技术 ， 抑 制 外 点 对 优化 估计 结果 的 影响 ， 单 一 视觉 SLAM 
在 KITTI 数据 集 上 的 平移 计算 精度 可 达 1%， 旋 转 误 差 低 于 
0.003 deg. m !. U571, 仍 难以 满足 汽车 自动 导航 等 大 尺度 环境 应 

迭代 求解 的 收敛 性 问题 引发 了 人 们 从 理论 层次 上 对 SLAM 
问题 的 深入 研究 ， 从 而 推动 了 算法 研究 的 进展 。Huang 等 人 最 
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早 对 SLAM 问题 的 非 凸 性 进行 了 研究 , 并 对 小 尺度 位 姿 图 优化 
进行 了 探讨 ?9。Knuth 等 人 研究 了 无 环 路 闭合 情况 下 状态 估 
计 误 差 的 增长 RU。Carlone 等 人 对 迭代 求解 的 收敛 性 问题 进行 
了 持续 研究 ， 首 先 给 出 了 Gauss-Newton 法 的 收敛 域 的 估计 方 
法 ， 并 证 明了 2D 场景 下 旋转 估计 的 封闭 性 与 唯一 性 ， 然 后 又 
提出 根据 SLAM 问题 的 强 对 偶 性 ,最 大 似 然 估 计 是 唯一 的 , 采 
用 半 正 定 规划 (semidefinite programming, SDP) 可 得 到 位 姿 图 的 
ERRES, Liu 等 人 采用 凸 松弛 法 避免 求解 陷入 局 部 极 
小 cg。 根据 SLAM 问题 非 线性 主要 来 源 于 旋转 计算 的 非 线性 ， 
Carlone 等 人 先进 行 旋转 估计 , 利用 估计 结果 加 速 非 线性 迭代 求 
解 效 果 P。 近 期 ， 为 制定 有 效 的 安全 紧要 场景 下 的 系统 故障 检 
测 与 失效 恢复 策略 ，Carlone 等 人 利用 SLAM 的 Lagrangian 对 
偶 性 ， 针 对 位 姿 估计 结果 的 评估 与 验证 ， 提 出 了 有 效 的 解决 方 
Sep), 
2.8 SLAM 问题 的 求解 

SLAM 待 估计 变量 包含 了 所 有 机 器 人 位 姿 与 观测 路 标点 。 
对 于 视觉 SLAM, 路 标点 的 数目 远大 于 位 姿 数 ，5X 维 数 达 到 数 
千 维 ， 使 得 增 量 方程 规模 巨大 。 根 据 传感器 的 视野 有 限 ， 可 观 
测 路 标 独 立 且 有 限 这 一 事实 ，SLAM 问题 具有 良好 的 稀 玻 性 ， 


并 可 以 用 因子 图 直观 表示 ， 如 图 5 所 示 。 
| 
u u / U3 
Xo X1 X3 
Z Zi Z Z3 Z4 Z5 Z6 | Z7 
L 


图 5 SLAM 因子 图 示意 图 


Fig.5 SLAM asa factory graph 

因子 图 是 一 种 二 维 无 向 图 ， 用 多 个 因子 的 概率 分 布 的 乘积 
估计 联合 概率 分 布 2。SLAM 因子 图 由 若干 个 节点 与 连接 节点 
的 边 构 成 ， 圆 形 节点 表示 待 优 化 状态 变量 ， 对 应 于 机 器 人 及 系 
统 在 不 同时 刻 的 状态 ,矩形 节点 表示 对 路 标的 观测 与 控制 输入 ， 
边 描述 了 相 邻 位 姿 变换 关系 或 对 路 标的 观测 误差 。 由 于 i 时 刻 
位 姿 对 J 路 标的 观测 产生 的 误差 项 与 其 他 时 刻 位 姿 与 路 标 无 关 
JETI EE FE EE JOO 中 仅 3e; ; / Ox; 5 0e, j /0y; 项 不 为 零 : 


Ce, . Ce, , 
J, =| 0---0 95; g..g Či 0...0 (11) 
Ox, Oy; 
与 图 4 对 应 的 雅 可 比 矩 阵 为 
| Jio Ji, 1 
Ja J» 
Jy J; 
J, Pi Ja , 
. 50 55 
Je|c [e J J (12) 
J 61 65 
Bi Jp J5 
Jo so 
Jy Jy 
L Jos deal 
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JOO SE H EUER AS did, ,岂可 表示 为 


H = David, (13) 
Xt H EG A BUE 
H, H 
"-| 11 i 14 
H, H,, ( ) 
Beh, Ha. WASARA, Hy =H. BTE 


标 数 远 远大 于 位 姿 数 , 采用 
姿 的 增 量 方程 


Schur 消 元 法 消去 Ho, WERF 


[H,, ~ H,H5H; 6x, = X. S H,Hjx, (15) 


其 中 ， X. =[X,…,X,] , X, =[Y Syl o 路 标的 增 量 方程 为 


Ox, = Hy (xX, —H),6x,) (16) 
J(x 
" (x) i 
米 米 0 
2| KK 米 米 米 米 
米 米 2| 米 米 米 米 
4| Æ 米 KKK KKK 
* * 4 米 米 米 
6| * 米 
米 米 6| 米 米 米 米 
8 米 米 米 米 
米 米 8 米 米 米 
10 * * | 
0 2 4 6 8 
0 2 4 6 8 nz= 28 
nz= 20 


图 6 雅 可 比 盾 阵 与 Hessian 矩阵 的 稀疏 性 
Fig.6 Sparsity of the Jacobian and Hessian matrix 

图 6 给 出 了 图 5 所 示 的 由 4 位 姿 与 4 路 标 构成 的 SLAM 问 
ALEK AE AY LE HERE JOO 与 Hessian 矩阵 H WER A. HFH 
(ARBRE, HIRERE Ho, 为 对 角 阵 ， 位 姿 增 量 方程 求解 中 ， 
Ha 易于 求解 ， 同 时 由 于 在 实际 问题 求解 过 程 中 ， 位 姿 数 远 小 
于 路 标 数 1 ，6X. 求解 规模 相对 于 原 问题 大 大 简化 。 

利用 Hessian 矩阵 的 稀 朴 性 ,采用 Schur 消 元 与 线性 分 解 ， 
然后 计算 增 量 方程 ， 显 著 提 高 了 SLAM 问题 优化 求解 的 效率 。 
前 , 已 涌现 出 众多 因子 图 求解 实现 框架 , 如 82oB94、TOROB1、 
HOG-ManB3、COP-SLAMB3] 等 ,在 普通 PC 处 理 器 已 可 实现 数 
于 维 变量 的 实时 求解 , 大 大 促进 了 SLAM 技术 从 研究 走向 应 用 
的 进程 。 


3 ”图 优化 SLAM 算法 实现 框架 


图 优化 技术 为 SLAM 系统 的 性 能 提供 了 理论 保证 ， 但 
SLAM 系统 的 实现 还 依赖 路 标 信息 感知 、 特 征 数据 关联 、 地 图 
表示 等 关键 技术 ， 并 与 程序 的 设计 等 工程 应 用 问题 紧密 相连 ， 
本 节 对 现 有 代表 性 的 基于 图 优化 的 SLAM 系统 实现 方案 与 性 
能 指标 进行 分 析 。 

1) PTAM 

PTAM 的 提出 是 视觉 SLAM 发 展 过 程 中 
事件 。PTAMB9 确 立 了 视觉 SLAM 的 基本 实 


Ñ 4 
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G I 
oe 
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大 创举 : 1) 首 次 采用 非 线 性 优化 技术 替代 传统 滤波 技术 作为 既 不 提取 描述 子 ， 也 不 处 理 稠密 或 半 稠 密 信息 ， 降 低 了 对 CPU 
SLAM 后 端的 实现 方案 ; 2) 引 入 关键 帧 机 制 ， 使 对 地 图 的 优化 计算 能 力 的 要 求 , 可 实现 在 无 人 机 、 手 持 AR/VR 等 设备 上 的 实 
可 以 整合 到 实时 计算 中 。 在 方案 实现 时 ， 设 计 2 个 独立 的 线程 ae 
分 别 实现 相机 位 姿 的 跟踪 与 地 图 的 构建 。 位 姿 跟 踪 线 程 实时 响 标点 的 位 置 估计 。 但 是 ， 由 于 不 包含 后 端 优化 和 环 路 检测 ， 
应 图 像 数据 ， 地 图 构建 线程 专注 于 地 图 的 建立 、 维 护 和 更 新 。 不 存在 建 图 功能 ，SVO 位 姿 估计 存在 累积 误差 ， ml 
于 仅 需 维护 视频 关键 帧 与 关键 帧 稳定 观测 的 地 标点 ， 使 得 目 难以 实现 重 定位 。 
标 函 数 的 优化 得 以 高 效 求 解 。 但 是 ， 当 地 图 构建 或 优化 过 慢 易 
导致 跟踪 线程 产生 跟踪 丢失 。Pire 等 采用 立体 相机 对 PTAM 技 
术 进 行 改进 ,将 立体 约束 用 于 路 标 特征 的 初始 化 跟踪 与 构图 ， 
在 系统 中 增加 了 实时 环 路 检测 与 修正 模块 ， 并 采用 局 部 并 行 光 
束 平 差 技术 (BA) 优 化 局 部 地 图 ， 实 现 了 实时 求解 ， 同 时 位 姿 估 
计 精 度 也 得 到 了 提升 551。 

2) ORB-SLAM 


ORB-SLAM 继承 了 PTAM 的 后 端 非 线性 


关键 帧 处 理 机 制 , 支持 单 
是 现代 SLAM 系统 中 最 为 完 


\ 双 目 .RGB-D 
善 、 易 用 的 


ORB 特征 


实现 目标 特征 


匹配 与 跟踪 ， 


计算 的 同时 又 兼 具 良好 的 旋转 与 缩放 不 变性 。 
的 实时 提取 与 离线 构建 的 ORB 字典 的 运用 ， 
标 重 定位 成 为 可 


的 回环 检测 与 目 


fig. ORB- 


三 线程 : 实时 特 


述 子 ， 粗 略 计算 路 标 位 置 与 相机 位 姿 ; 


征 跟踪 线程 根据 关键 帧 匹配 路 标 ORB 特 


在 普通 CPU 上 实现 实时 


优化 实现 方案 与 
三 种 视频 输入 模式 ， 
系统 之 一 66371。 采 用 


ORB 特征 描述 子 
使 大 尺度 运动 时 
SLAM 代码 实现 采用 
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Fig.7 Feature-based map vs. semi-dense reconstruction 


5) SOFT 


SOFT 采用 双 目 立体 相机 ， 


局 环 路 检测 与 优 


图 构建 模块 失效 时 的 定位 漂移 问题 ， 定 他 


化 线程 消除 状态 估计 累积 误差 ， 获 取 全 局 一 


致 运动 轨迹 估计 。 


tt ORB-SLAM, ORB-SLAM2 增加 了 地 图 重 


功能 ,解决 地 


M. 


作 时 的 稳健 型 也 得 以 提高 69。 
3) LSD-SLAM: 
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利用 极 线 上 等 距 


地 图 ， 避 
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丢失 ， 此 外 ， 在 了 


漂移 问题 ， 利 用 
强烈 差异 时 ， 光 
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深度 夹 角 关系 归纳 成 光度 不 确定 相 ; 在 后 端 优化 中 考虑 


量 深度 不 确定 


FE 跟踪 的 稳定 性 ; 在 深 
完成 后 进行 深度 均 
何 关系 与 


性 时 ， 将 几 


减 小 


仿 射 


尺度 漂移 。 直 
内 参 和 了 曝光 非常 敏感 ， 在 相机 快速 移动 时 容易 
不 路 检测 时 ， 仍 依赖 于 特征 
SLAM 采用 双 目 立体 相机 估计 场景 深度 ， 解 决 单 目 相机 的 
光照 校正 技术 使 得 在 相 邻 关键 


接 法 跟 


以 不 变 [40]。 


图 7 为 特征 

中 的 梯度 差异 

进行 显示 ， 比 稀 
4) SVO 


Ri 


SVO 综合 使 用 直 


区 


建 模 ， 


度 残 差 近 
也 图 与 半 稠 密 重建 对 照 。 


踪 对 特征 缺失 区 域 不 


正点 的 计算 。S-LSD- 
尺度 
WIE HR FEE 


半 笛 密 地 图 根据 灰 度 


将 物体 的 边缘 或 表 


面 纹理 部 分 在 地 图 


琉 地 图 


kt 有 更 多 的 信息 。 


楼 法 与 特征 


ERR SVO | 


mH 


正点 周围 


法 计算 路 标 特征 
4x4 图 像 块 估计 相机 位 姿 与 路 标 位 置 ， 


与 实现 目标 


精细 
提取 Blob 特征 


与 


选择 减少 定位 漂移 中 1。 


SOFT 


角 点 特征 


间 的 对 应 关系 ， 


点 法 与 RANSAC 
匹配 误差 。 此 外 ,1 


可 在 ARM 平台 上 实现 高 精 


采用 归 一 


通过 对 稳定 跟踪 特征 
在 当前 帧 的 一 个 小 的 窗 
,通过 非 极 大 值 抑制 技术 
化 互相 关 去 除外 点 ， 确 定 稳定 匹配 
特征 。 将 运动 估计 分 为 旋转 与 平移 两 个 部 分 ， 旋 转 估计 采用 5 


的 子 集 的 
口内 


判断 特征 点 


4, 平移 估计 采用 最 小 重 投 影 误 差 技 术 , 减少 


IMU 信 


还 可 采用 


息 抑 制 外 点 与 优化 旋转 估计 ， 
度 位 姿 的 实时 估计 。 


上 述 5 种 算法 的 位 姿 估 计 精 度 与 实时 性 对 比如 表 1 所 示 。 


表 1 SLAM 实现 方案 性 能 对 


t 


Table 1 Performance comparison of SLAM algorithms 


位 姿 估计 精度 


实现 方案 《一 每 帧 运行 时 间 (s) 运行 环境 

平移 ”旋转 (deg/m) 

S-PTAM 135% 0.0023 0.08 4 cores, 2.2GHz, C/C++ 
ORB-SLAM2 1.15% 0.0027 0.06 2 cores, 3.5GHz, C/C++ 
S-LSD-SLAM 1.20% 0.0033 0.07 1 cores, 3.5GHz, C/C++ 

SVO2 0.94% 00021 02 1 cores, 2.5GHz, C/C++ 
SOFT 0.88% 0.0022 0.1 2 cores, 2.5GHz, C/C++ 
4 MARRS 
1) 算法 稳健 型 
算法 稳健 性 是 视觉 SLAM 系统 全 生命 周期 工作 需 解决 的 
首要 问题 ,算法 设计 的 局 限 性 与 硬件 相关 问题 , 如 传感器 失效 、 


H ZE 3L 


驱动 器 误差 等 ， 使 得 当 
硬件 平台 上 达到 特定 的 性 


前 


SLAM 系统 只 能 大 


AL BE RES), 


E 特 定 环境 下 、 特 定 
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在 众多 影响 视觉 SLAM 系统 稳定 工作 的 因素 中 , 数据 关联 ”等 通过 对 已 有 节点 引入 新 的 约束 尽量 避免 新 的 节点 的 添加 ， 使 
的 计算 涉及 到 传感器 测量 之 间 、 传 感 器 测量 与 地 图 特征 之 间或 得 因子 图 的 扩展 只 与 机 器 人 探索 区 域 有 关 ， 而 与 工作 时 间 无 关 
地 图 特征 之 间 的 对 应 关系 , 直接 决定 了 SLAM 问题 求解 的 准确 1, Kretzschmar 等 针对 位 姿 图 优化 问题 , 研究 了 基于 信息 论 的 
性 与 实时 性 。 对 于 错误 数据 关联 导致 的 算法 失效 可 以 从 SLAM ” 节点 与 边 的 边缘 化 准则 [66]。Carlevaris 与 Mazuran 等 介绍 了 广 
系统 前 端 与 后 端 两 个 方面 入 手 加 以 解决 。 在 前 端 ， 满 足 传感器 。 义 线性 约束 因子 与 相应 的 非 线性 图 稀 玻 化 方法 [6 人 1。 实 现 因子 
采样 速率 远 高 于 机 器 人 运动 姿态 变化 的 情况 下 ， 特 征 描 述 子 法 ” 图 稀 纹 化 的 另 一 个 途径 是 通过 对 连续 时 间 运 动 轨迹 的 估计 减少 
与 光 流 法 [9 均 可 以 有 效 的 实现 关键 帧 观测 路 标的 准确 跟踪 。 为 待 估计 参数 ， 如 利用 三 次 样 条 曲线 、B 样 条 曲线 等 ， 采 用 滑动 
降低 累积 估计 误差 ， 需 将 当前 观测 数据 与 历史 观测 数据 实时 关  ” 窗口 或 批 处 理 模式 表示 机 器 人 运动 路 径 [9"。Chi 等 用 高 斯 过 
联 ， 实 现 环 路 检测 。 词 袋 法 9 是 目前 最 为 有 效 的 实现 环 路 检测 ，” 程 表 示 取 代 基 本 样 条 曲线 表示 法 ， 稀 玻 化 因子 图 中 的 节点 是 真 
技术 之 一 ， 可 显著 增强 变量 估计 的 一 致 性 。 其 基本 思想 是 从 图 。” 实 的 机 器 人 位 姿 ， 其 他 位 姿 可 通过 计算 给 定时 间 的 后 验 均值 的 
像 中 提取 的 局 部 特征 进行 聚 类 ， 将 连续 变化 特征 量化 为 离散 的 ”插值 获得 [4。 
单词 ， 然 后 采用 词 的 统计 直方 图 对 场景 进行 描述 。 在 算法 实现 子 图 处 理 技术 将 因子 图 分 解 为 多 个 子 图 ， 利 用 多 个 处 理 器 
上 则 通过 应 用 分 层 词 汇 树 提高 大 规模 数据 集中 的 特征 检索 效率 ， 分布 式 计算 局 部 因子 图 优化 进而 实现 全 局 因子 图 的 优化 [51。 
实现 大 尺度 环境 实时 环 路 检测 65153。 但 对 于 存在 强烈 光照 变化 Ni 等 提出 基于 二 进 制 树 结构 的 子 图 构建 与 组 织 方法 ， 
的 场景 ， 视 觉 字 的 准确 匹配 难以 实现 531。 将 不 同 视 觉 外 观 纳入 ”Grisetti05] 对 子 图 进行 等 级 划分 ， 新 的 观测 仅 更 新 最 高 等 级 与 低 
统一 表征 框架 5 约 ， 或 者 采用 路 标 视觉 外 观 信息 与 空间 位 置 关 系 ”等 级 受 影响 区 域 。 将 大 尺度 场景 分 解 成 多 个 小 的 区 域 ， 利 用 多 
融合 63， 降低 光照 变化 对 场景 识别 的 影响 ， 增 强 环 路 检测 的 稳 “个 机 器 人 分 别 进行 构图 为 子 图 处 理 技术 提供 了 另 一 种 思路 6771。 
健 性 ， 是 最 近 研究 的 热点 问题 。 文 献 59 对 视觉 场景 识别 方法 进 ” 多 机 器 人 构图 又 可 分 为 中 心 融 合 处 理 与 分 布 式 处 理 两 类 。 中 心 
行 了 详细 的 归纳 与 总 结 。 融合 处 理 采 用 中 心 处 理 单元 对 子 图 信息 进行 融合 ， 而 分 布 式 处 
尽管 前 端的 环 路 检测 技术 取得 了 长 是 进步 ， 但 视觉 混 车 导 里 通过 机 器 人 间 相 互通 信保 持 建 图 信息 的 一 致 性 。 文 献 SU 对 多 
致 的 环 路 检测 错误 对 后 端 估计 失效 的 影响 仍 难以 避免 ， 增 强 后 。 机 器 人 构图 技术 进行 了 详细 的 分 析 。 
端 算法 对 虚假 数据 关联 的 抵御 能 力 显 得 至 关 重 要 。 常 用 的 解决 前 ，SLAM 算法 可 扩展 性 研究 仍 集中 于 简化 因子 图 优化 
途径 包括 : 1) 利 用 先 验证 知识 ， 检 测 错误 的 闭合 环 路 ， 在 进行 ”的 复杂 度 ， 对 机 器 人 大 尺度 环境 工作 涉及 的 其 他 众多 问题 ， 如 
优化 计算 之 前 抑制 外 点 的 影响 ，2) 根 据 优化 估计 残 差 验 证 环 路 。 ”语义 地 图 构建 、 分 布 式 建 图 的 稳健 性 与 通过 构图 实现 环境 的 深 


am 


检测 的 正确 性 S71。 度 感 知 使 机 器 人 具有 类 人 智能 等 的 研究 还 有 待 进一步 展开 。 
基于 图 优化 的 SLAM 问题 的 本 质 是 求解 非 线 性 、 非 凸 优化 3) 深度 学 习 
问题 ， 算 法 求解 器 对 于 初始 值 非常 敏感 ， 且 极 易 陷入 局 部 极 小 深度 学 习 技术 的 发 展 引起 了 计算 机 视觉 领域 的 一 场 变革 。 


点 ， 导 致 估计 结果 存在 巨大 偏差 。 理 想 的 SLAM 系统 应 能 实时 前 , 利用 深度 学 习 技术 解决 SLAM 求解 问题 的 研究 已 经 展开 。 
评估 算法 估计 结果 ， 并 有 具有 从 算法 失效 状态 实现 自我 恢复 的 能 Costante 等 采用 表示 学 习 方法 取代 视觉 里 程 计 中 的 几何 约束 ， 
JIo 紧密 整 合 SLAM 前 端 与 后 端 , 是 提高 SLAM 算法 稳健 性 的 根据 相 邻 两 帧 图 像 输 入 估计 机 器 人 位 姿 中 时。Eigen 等 通过 对 路 
有 效 解决 途径 ， 但 相关 研究 工作 还 有 待 进一步 展开 。 此 外 ， 利 标点 赋予 语义 信息 ， 联 合 利 用 路 标点 的 位 置信 息 与 语义 信息 提 
用 视觉 、IMU、GNSS、3D 激光 探测 器 构建 定制 地 图 ， 并 实现 ”高 数据 关联 的 正确 率 ， 从 而 改进 位 姿 与 路 标点 位 置 的 估计 精度 
在 已 有 地 图 中 的 重 定 位 技术 , 增强 SLAM 技术 在 特定 场景 的 应 [523。 刘 等 利用 深度 学 习 网 络 ， 实 现 了 基于 单 帧 图 像 的 场景 深度 
用 ,如 自动 驾驶 等 ， 也 是 当前 SLAM 技术 研究 的 热点 问题 之 一 估计 Fo50。 此 外 ， 语 义 信息 的 引入 为 环 路 检测 、 地 图 表示 等 带 


ren, 来 更 多 的 条 件 [2s31。 
2) 可 扩展 性 通过 构建 复杂 的 CNN 网 络 ， 深 度 学 习 可 实现 目标 特征 的 


可 扩展 性 是 SLAM 系统 实现 长 时 间 、 大 尺度 应 用 ， 如 自动 ”多 层次 提取 ， 进 而 实现 环境 信息 的 深度 感知 ， 而 传统 计算 机 视 
驾驶 、 海 底 环境 检测 、 大 规模 精准 农业 等 ， 需 解决 的 另 一 个 重 。 觉 技术 对 环境 的 认 知 仍 停留 在 像素 或 特征 点 等 浅 层次 特征 提取 
要 问题 。 随 着 工作 时 间 的 延长 与 探索 范围 的 拓展 ，SLAM 位 姿 上 ， 因 此 ， 作 为 感知 工具 ， 深 度 学 习 技 术 可 以 解决 传统 计算 机 
图 规模 将 无 限 增加 而 线性 迭代 求解 对 存储 资源 的 需求 与 待 估计 ”视觉 技术 无 法 解决 的 问题 。 但 是 对 于 SLAM 技术 , 环境 感知 是 
变量 的 数量 成 正比 ， 导 致 系统 不 可 实现 [931]。 降 低 因子 图 优化 的 ”为 定位 与 构图 服务 的 ， 是 否 能 用 深度 学 习 技术 实现 端 到 端的 
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复杂 度 ， 使 得 问题 求解 对 计算 与 存储 资源 的 需求 保持 恒定 ， 主 SLAM 系统 仍 处 于 探索 之 中 。 此 外 ， 利 用 场景 先 验 信息 可 以 显 
要 途径 是 稀 琉 化 方法 与 子 图 处 理 技 术 。 著 提 升 SLAM 系统 的 性 能 , 但 将 深度 学 习 网 络 输出 的 不 确定 性 
基于 Markov 毯 理 论 ， 通 过 边 与 节点 的 边缘 化 可 获得 因子 。 对 SLAM 视觉 几何 处 理 的 影响 的 研究 还 有 待 展开 。 
RA. La 等 采用 信息 论 方法 控制 因子 图 中 节点 与 边 未 来 的 SLAM 系统 工作 于 开放 的 环境 , 需要 对 环境 进行 持 
兽 添 ， 仅 增加 非 元 余 节 点 与 信息 量 含量 高 的 边 ( 鸣 。Jonhannsson ” 续 的 探索 ， 因 此 应 具有 全 生命 周期 的 学 习 能 力 ， 而 深度 学 习 的 


LR, 


成 功 应 用 依赖 于 对 固定 目标 类 的 大 量 数据 与 标注 样本 的 训练 ， 
因此 ， 改 进深 度 学 习 技术 的 在 线 学 习 与 自 适应 能 力 ， 应 是 实现 
具有 类 人 智能 的 SLAM 系统 的 首要 解决 问题 之 一 。 此外， 开发 
轻 量 级 的 网 络 , 满足 SLAM RRRA mR, EIRE 
学 习 与 SLAM 技术 的 结合 带 来 了 诸多 挑战 。 


5 ”结束 语 


n 


本 文 回 顾 了 SLAM 技术 近 三 十 年 的 发 展 历程 , 详细 分 析 了 
基于 图 优化 的 视觉 SLAM 的 理论 模型 与 算法 实现 , 探讨 了 问题 
求解 中 存在 的 问题 并 给 出 了 最 新 的 理论 研究 成 果 。 目 前 ， 
SLAM 理论 研究 已 趋 于 完善 ， 满 足 大 不 境 定位 与 构图 的 算 
去 框架 相继 被 提出 ， 但 针对 全 生命 周期 、 大 环境 尺度 工作 应 用 
需求 ， 其 算法 的 稳健 性 与 可 扩展 性 设计 仍 面 | kiko T4 
建 大 尺度 、 全 自主 运行 的 SLAM 系统 ， 满 足 自动 驾驶 、 海 洋 测 
绘 、 大 规模 精准 农业 等 复杂 场景 应 用 是 现代 SLAM 技术 发 展 的 
标 , 将 传统 SLAM 技术 与 深度 学 习 等 新 技术 结合 建立 对 工作 


a 
E 


ae 


环境 的 语义 层次 的 深度 感知 ， 同 时 融合 GNSS、 人 惯性 测量 、 激 
光 雷 达 等 传 感 技 术 ， 增 强 系统 复杂 环境 下 工作 的 稳健 性 ， 则 为 
该 目标 的 实现 提供 了 有 效 地 解决 途径 。 
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